﻿#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <iomanip>
#include <tuple>
#include <limits>

using namespace std;


struct Point {
    double x;
    double y;
    Point(double x_, double y_) : x(round(x_ * 10000) / 10000),
        y(round(y_ * 10000) / 10000) {}
};

/**
 * @brief 三边定位算法实现
 *
 * @param x1 第一个参考点x坐标
 * @param y1 第一个参考点y坐标
 * @param d1 到第一个参考点的距离
 * @param x2 第二个参考点x坐标
 * @param y2 第二个参考点y坐标
 * @param d2 到第二个参考点的距离
 * @param x3 第三个参考点x坐标
 * @param y3 第三个参考点y坐标
 * @param d3 到第三个参考点的距离
 * @return vector<Point> 可能的解集合（已排序）
 *
 * @throws invalid_argument 当输入参数非法时抛出
 */
vector<Point> trilaterate(
    double x1, double y1, double d1,
    double x2, double y2, double d2,
    double x3, double y3, double d3) {

    const double epsilon = 1e-6;
    vector<Point> solutions;

    // 输入验证
    auto validate = [&](double d) {
        if (d < 0) throw invalid_argument("距离不能为负");
        if (!isfinite(d)) throw invalid_argument("非法数值");
        };

    validate(d1); validate(d2); validate(d3);
    if (!isfinite(x1) || !isfinite(y1) ||
        !isfinite(x2) || !isfinite(y2) ||
        !isfinite(x3) || !isfinite(y3)) {
        throw invalid_argument("坐标包含非法数值");
    }

    // 辅助函数：计算两点距离平方
    auto dist_sq = [](double x1, double y1, double x2, double y2) {
        return pow(x1 - x2, 2) + pow(y1 - y2, 2);
        };

    // 处理前两个圆
    double dx = x2 - x1;
    double dy = y2 - y1;
    double d = sqrt(dx * dx + dy * dy);

    // 计算前两个圆的交点
    if (d > epsilon) {
        double r_sum = d1 + d2;
        double r_diff = abs(d1 - d2);

        if (d <= r_sum + epsilon && d >= r_diff - epsilon) {
            double a = (pow(d1, 2) - pow(d2, 2) + pow(d, 2)) / (2 * d);
            double h = sqrt(pow(d1, 2) - pow(a, 2));

            if (!isnan(h)) {
                // 计算交点坐标
                double px = x1 + a * dx / d;
                double py = y1 + a * dy / d;

                double dx_perp = h * dy / d;
                double dy_perp = -h * dx / d;

                vector<Point> candidates = {
                    {px + dx_perp, py + dy_perp},
                    {px - dx_perp, py - dy_perp}
                };

                // 验证第三个圆
                for (auto& p : candidates) {
                    double error = max(1e-6, 0.001 * d3);
                    double actual = sqrt(dist_sq(p.x, p.y, x3, y3));
                    if (abs(actual - d3) <= error) {
                        solutions.push_back(p);
                    }
                }
            }
        }
    }

    // 去重处理
    sort(solutions.begin(), solutions.end(), [](const Point& a, const Point& b) {
        return abs(a.x - b.x) < 1e-6 ? (a.y < b.y) : (a.x < b.x);
        });
    auto last = unique(solutions.begin(), solutions.end(), [](const Point& a, const Point& b) {
        return abs(a.x - b.x) < 1e-6 && abs(a.y - b.y) < 1e-6;
        });
    solutions.erase(last, solutions.end());

    // 最小二乘法处理无解情况
    if (solutions.empty()) {
        // 构建线性方程组
        auto build_eq = [](double xa, double ya, double da,
            double xb, double yb, double db) {
                return make_tuple(
                    2 * (xb - xa),
                    2 * (yb - ya),
                    pow(da, 2) - pow(db, 2) + pow(xb, 2) + pow(yb, 2) - pow(xa, 2) - pow(ya, 2)
                );
            };

        auto [A1, B1, C1] = build_eq(x1, y1, d1, x2, y2, d2);
        auto [A2, B2, C2] = build_eq(x2, y2, d2, x3, y3, d3);

        // 解方程组
        double det = A1 * B2 - A2 * B1;
        if (abs(det) > epsilon) {
            double x = (B2 * C1 - B1 * C2) / det;
            double y = (A1 * C2 - A2 * C1) / det;

            // 验证所有圆
            bool valid = true;
            valid &= abs(sqrt(dist_sq(x, y, x1, y1)) - d1) <= max(1e-6, 0.001 * d1);
            valid &= abs(sqrt(dist_sq(x, y, x2, y2)) - d2) <= max(1e-6, 0.001 * d2);
            valid &= abs(sqrt(dist_sq(x, y, x3, y3)) - d3) <= max(1e-6, 0.001 * d3);

            if (valid) solutions.emplace_back(x, y);
        }
    }

    // 最终排序
    sort(solutions.begin(), solutions.end(), [](const Point& a, const Point& b) {
        return abs(a.x - b.x) < 1e-6 ? (a.y < b.y) : (a.x < b.x);
        });

    return solutions;
}

// 单元测试
void runTests() {
    auto test = [](const char* name, auto&& func) {
        cout << "测试用例: " << name;
        try { func(); cout << " ✓ 通过" << endl; }
        catch (exception& e) { cout << " × 失败: " << e.what() << endl; }
        };

    test("标准情况（唯一解）", [] {
        auto res = trilaterate(0, 0, 4, 8, 0, 4, 4, 8, 8);
        if (res.size() != 1) throw runtime_error("解数量错误");
        if (abs(res[0].x - 4.0) > 1e-4 || abs(res[0].y - 0.0) > 1e-4)
            throw runtime_error("解坐标错误");
        });

    test("两个解情况", [] {
        auto res = trilaterate(0, 0, 5, 6, 0, 5, 3, 0, 4);
        if (res.size() != 2) throw runtime_error("解数量错误");
        if (abs(res[0].y - 4.0) > 1e-4 || abs(res[1].y + 4.0) > 1e-4)
            throw runtime_error("解坐标错误");
        });

    test("无解情况", [] {
        auto res = trilaterate(0, 0, 2, 10, 0, 2, 5, 5, 10);
        if (!res.empty()) throw runtime_error("应返回空列表");
        });
}

//int main() {
//    try {
//        runTests();
//        cout << "\n所有测试通过！" << endl;
//    }
//    catch (...) {
//        cout << "\n测试未全部通过！" << endl;
//    }
//    return 0;
//}


int main() {
    cout << "=== 三边定位计算器 ===" << endl;
    cout << "格式说明：输入每个参考点的x坐标、y坐标和距离，用空格分隔" << endl;

    while (true) {
        // 输入三个参考点数据
        vector<double> inputs(9);
        const char* prompts[] = {
            "第一个参考点 (x1 y1 d1): ",
            "第二个参考点 (x2 y2 d2): ",
            "第三个参考点 (x3 y3 d3): "
        };

        // 处理用户输入
        bool input_valid = true;
        for (int i = 0; i < 3; ++i) {
            cout << prompts[i];
            double x, y, d;

            // 增强输入验证
            if (!(cin >> x >> y >> d)) {
                cin.clear();
                cin.ignore(numeric_limits<streamsize>::max(), '\n');
                cout << "! 输入格式错误，请重新输入三个数值" << endl;
                input_valid = false;
                break;
            }

            inputs[i * 3] = x;
            inputs[i * 3 + 1] = y;
            inputs[i * 3 + 2] = d;
        }

        if (!input_valid) continue;

        // 执行计算
        try {
            auto result = trilaterate(
                inputs[0], inputs[1], inputs[2],
                inputs[3], inputs[4], inputs[5],
                inputs[6], inputs[7], inputs[8]
            );

            // 格式化输出结果
            cout << "\n计算结果：";
            if (result.empty()) {
                cout << "无解" << endl;
            }
            else {
                cout << "找到 " << result.size() << " 个解" << endl;
                for (auto& p : result) {
                    cout << fixed << setprecision(4)
                        << "(" << p.x << ", " << p.y << ")" << endl;
                }
            }
        }
        catch (const invalid_argument& e) {
            cout << "计算错误: " << e.what() << endl;
        }

        // 继续控制
        cout << "\n是否继续计算？(y/n) ";
        char choice;
        cin >> choice;
        cin.ignore(numeric_limits<streamsize>::max(), '\n');

        if (tolower(choice) != 'y') {
            cout << "=== 感谢使用 ===" << endl;
            break;
        }
        cout << endl;
    }

    return 0;
}